{
  "cells": [
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "%matplotlib inline"
      ]
    },
    {
      "source": [
        "\n********************************************************************************\nNavigation on Flat Earth - Benchmark\n********************************************************************************\nGoals of this script:\n\n- implement different UKFs on the navigation on flat Earth example.\n\n- design the Extended Kalman Filter (EKF) and the Invariant Extended. Kalman\n  Filter (IEKF) :cite:`barrauInvariant2017`.\n\n- compare the different algorithms with Monte-Carlo simulations.\n\n*We assume the reader is already familiar with the considered problem described\nin the related example.*\n\nThis script searches to estimate the 3D attitude, the velocity, and the position\nof a rigid body in space from inertial sensors and relative observations of\npoints having known locations. For the given problem, three different UKFs\nemerge, defined respectively as:\n\n1) The state is embedded  in $SO(3) \\times \\mathbb{R}^6$, i.e.\n\n   - the retraction $\\varphi(.,.)$ is the $SO(3)$ exponential for\n     orientation, and the vector addition for robot velocity and\n     position.\n\n   - the inverse retraction $\\varphi^{-1}(.,.)$ is the $SO(3)$\n     logarithm for orientation and the vector subtraction for velocity\n     and position.\n\n2) The state is embedded in $SE_2(3)$ with left multiplication, i.e.\n\n   - the retraction $\\varphi(.,.)$ is the $SE_2(3)$ exponential,\n     where the state multiplies on the left the uncertainty\n     $\\boldsymbol{\\xi}$.\n\n   - the inverse retraction $\\varphi^{-1}(.,.)$ is the $SE_2(3)$\n     logarithm.\n\n3) The state is embedded in $SE_2(3)$ with right multiplication, i.e.\n\n   - the retraction $\\varphi(.,.)$ is the $SE_2(3)$ exponential,\n     where the state multiplies on the right the uncertainty \n     $\\boldsymbol{\\xi}$.\n\n   - the inverse retraction $\\varphi^{-1}(.,.)$ is the $SE_2(3)$\n     logarithm.\n\n   - this right UKF corresponds to the Invariant Extended Kalman Filter (IEKF)\n     recommended in :cite:`barrauInvariant2017`.\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>The exponential and logarithm of $SE_2(3)$ are quickly derived from \n    the $SE(3)$ exponential and logarithm, see Lie Groups documentation.</p></div>\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "Import\n==============================================================================\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "from ukfm import SO3, UKF, EKF\nfrom ukfm import INERTIAL_NAVIGATION as MODEL\nfrom scipy.linalg import block_diag\nimport numpy as np\nimport matplotlib\nimport ukfm\nukfm.set_matplotlib_config()"
      ]
    },
    {
      "source": [
        "Simulation Setting\n==============================================================================\nWe compare the filters on a large number of Monte-Carlo runs.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# Monte-Carlo runs\nN_mc = 100"
      ]
    },
    {
      "source": [
        "The vehicle drives a 10-meter diameter circle in 30 seconds and observes three\nfeatures  every second while receiving high-frequency inertial measurements\n(100 Hz).\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# sequence time (s)\nT = 30\n# IMU frequency (Hz)\nimu_freq = 100\n# create the model\nmodel = MODEL(T, imu_freq)\n# observation frequency (Hz)\nobs_freq = 1\n# IMU noise standard deviation (noise is isotropic)\nimu_std = np.array([0.01,   # gyro (rad/s), not  0.6 deg/s\n                    0.01])  # accelerometer (m/s^2)\n# observation noise standard deviation (m)\nobs_std = 0.1"
      ]
    },
    {
      "source": [
        "Filter Design\n==============================================================================\nAdditionally to the three UKFs, we compare them to an EKF and an IEKF. The EKF\nhas the same uncertainty representation as the UKF with $SO(3) \\times\n\\mathbb{R}^6$ uncertainty representation, whereas the IEKF has the same\nuncertainty representation as the UKF with right $SE_2(3)$ retraction.\nAs we have five similar methods, the code is redundant.\n\nAll the filters have the same parameters.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# propagation noise covariance matrix\nQ = block_diag(imu_std[0]**2*np.eye(3), imu_std[1]**2*np.eye(3))\n# measurement noise covariance matrix\nR = obs_std**2 * np.eye(3*model.N_ldk)\n# initial uncertainty matrix such that the state is not perfectly initialized\nRot0_std = 15/np.sqrt(3)*np.pi/180\np0_std = 1/np.sqrt(3)\nP0 = block_diag(Rot0_std**2*np.eye(3), np.zeros((3, 3)), p0_std**2 * np.eye(3))\n# sigma point parameter\nalpha = np.array([1e-3, 1e-3, 1e-3])"
      ]
    },
    {
      "source": [
        "We set variables for recording metrics before launching Monte-Carlo\nsimulations.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "ukf_err = np.zeros((N_mc, model.N, 9))\nleft_ukf_err = np.zeros_like(ukf_err)\nright_ukf_err = np.zeros_like(ukf_err)\niekf_err = np.zeros_like(ukf_err)\nekf_err = np.zeros_like(ukf_err)\n\nukf_nees = np.zeros((N_mc, model.N, 2))\nleft_ukf_nees = np.zeros_like(ukf_nees)\nright_ukf_nees = np.zeros_like(ukf_nees)\niekf_nees = np.zeros_like(ukf_nees)\nekf_nees = np.zeros_like(ukf_nees)"
      ]
    },
    {
      "source": [
        "Monte-Carlo Runs\n==============================================================================\nWe run the Monte-Carlo through a for loop.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "for n_mc in range(N_mc):\n    print(\"Monte-Carlo iteration(s): \" + str(n_mc+1) + \"/\" + str(N_mc))\n    # simulate true states and noisy inputs\n    states, omegas = model.simu_f(imu_std)\n    # simulate measurements\n    ys, one_hot_ys = model.simu_h(states, obs_freq, obs_std)\n    # initialize filters\n    state0 = model.STATE(\n        Rot=states[0].Rot.dot(SO3.exp(Rot0_std*np.random.randn(3))),\n        v=states[0].v,\n        p=states[0].p + p0_std*np.random.randn(3))\n    # IEKF and right UKF covariance need to be turned\n    J = np.eye(9)\n    J[6:9, :3] = SO3.wedge(state0.p)\n    right_P0 = J.dot(P0).dot(J.T)\n    ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n              phi=model.phi, phi_inv=model.phi_inv, alpha=alpha)\n    left_ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n                   phi=model.left_phi, phi_inv=model.left_phi_inv, alpha=alpha)\n    right_ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n                    phi=model.right_phi, phi_inv=model.right_phi_inv,\n                    alpha=alpha)\n    iekf = EKF(model=model, state0=state0, P0=right_P0, Q=Q, R=R,\n               FG_ana=model.iekf_FG_ana, H_ana=model.iekf_H_ana,\n               phi=model.right_phi)\n    ekf = EKF(model=model, state0=state0, P0=right_P0, Q=Q, R=R,\n              FG_ana=model.ekf_FG_ana, H_ana=model.ekf_H_ana,\n              phi=model.phi)\n\n    ukf_states = [state0]\n    left_ukf_states = [state0]\n    right_ukf_states = [state0]\n    iekf_states = [state0]\n    ekf_states = [state0]\n\n    ukf_Ps = np.zeros((model.N, 9, 9))\n    left_ukf_Ps = np.zeros_like(ukf_Ps)\n    right_ukf_Ps = np.zeros_like(ukf_Ps)\n    ekf_Ps = np.zeros_like(ukf_Ps)\n    iekf_Ps = np.zeros_like(ukf_Ps)\n\n    ukf_Ps[0] = P0\n    left_ukf_Ps[0] = P0\n    right_ukf_Ps[0] = right_P0\n    ekf_Ps[0] = P0\n    iekf_Ps[0] = right_P0\n\n    # measurement iteration number\n    k = 1\n    # filtering loop\n    for n in range(1, model.N):\n        # propagation\n        ukf.propagation(omegas[n-1], model.dt)\n        left_ukf.propagation(omegas[n-1], model.dt)\n        right_ukf.propagation(omegas[n-1], model.dt)\n        iekf.propagation(omegas[n-1], model.dt)\n        ekf.propagation(omegas[n-1], model.dt)\n        # update only if a measurement is received\n        if one_hot_ys[n] == 1:\n            ukf.update(ys[k])\n            left_ukf.update(ys[k])\n            right_ukf.update(ys[k])\n            iekf.update(ys[k])\n            ekf.update(ys[k])\n            k += 1\n        # save estimates\n        ukf_states.append(ukf.state)\n        left_ukf_states.append(left_ukf.state)\n        right_ukf_states.append(right_ukf.state)\n        iekf_states.append(iekf.state)\n        ekf_states.append(ekf.state)\n        ukf_Ps[n] = ukf.P\n        left_ukf_Ps[n] = left_ukf.P\n        right_ukf_Ps[n] = right_ukf.P\n        iekf_Ps[n] = iekf.P\n        ekf_Ps[n] = ekf.P\n    #\u00a0get state\n    Rots, vs, ps = model.get_states(states, model.N)\n    ukf_Rots, ukf_vs, ukf_ps = model.get_states(ukf_states,  model.N)\n    left_ukf_Rots, left_ukf_vs, left_ukf_ps = model.get_states(\n        left_ukf_states,  model.N)\n    right_ukf_Rots, right_ukf_vs, right_ukf_ps = model.get_states(\n        right_ukf_states,  model.N)\n    iekf_Rots, iekf_vs, iekf_ps = model.get_states(iekf_states,  model.N)\n    ekf_Rots, ekf_vs, ekf_ps = model.get_states(ekf_states,  model.N)\n\n    # record errors\n    ukf_err[n_mc] = model.errors(Rots, vs, ps, ukf_Rots, ukf_vs, ukf_ps)\n    left_ukf_err[n_mc] = model.errors(\n        Rots, vs, ps, left_ukf_Rots, left_ukf_vs, left_ukf_ps)\n    right_ukf_err[n_mc] = model.errors(\n        Rots, vs, ps, right_ukf_Rots, right_ukf_vs, right_ukf_ps)\n    iekf_err[n_mc] = model.errors(Rots, vs, ps, iekf_Rots, iekf_vs, iekf_ps)\n    ekf_err[n_mc] = model.errors(Rots, vs, ps, ekf_Rots, ekf_vs, ekf_ps)\n\n    # record NEES\n    ukf_nees[n_mc] = model.nees(ukf_err[n_mc], ukf_Ps, ukf_Rots, ukf_vs,\n                                ukf_ps, 'STD')\n    left_ukf_nees[n_mc] = model.nees(left_ukf_err[n_mc], left_ukf_Ps,\n                                     left_ukf_Rots, left_ukf_vs, left_ukf_ps, \n                                     'LEFT')\n    right_ukf_nees[n_mc] = model.nees(right_ukf_err[n_mc], right_ukf_Ps,\n                                      right_ukf_Rots, right_ukf_vs, \n                                      right_ukf_ps, 'RIGHT')\n    iekf_nees[n_mc] = model.nees(iekf_err[n_mc], iekf_Ps, iekf_Rots, iekf_vs,\n                                 iekf_ps, 'RIGHT')\n    ekf_nees[n_mc] = model.nees(ekf_err[n_mc], ekf_Ps, ekf_Rots, ekf_vs,\n                                ekf_ps, 'STD')"
      ]
    },
    {
      "source": [
        "Results\n==============================================================================\nWe first visualize the trajectory results for the last run, where the vehicle\nstarts in the above center of the plot. As the simulation has random process,\nthe plot gives us an indication but not a proof of performances. We\nthen plot the orientation and position errors averaged over Monte-Carlo.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "ukf_err, left_ukf_err, right_ukf_err, iekf_err, ekf_err = model.benchmark_plot(\n    ukf_err, left_ukf_err, right_ukf_err, iekf_err, ekf_err, ps, ukf_ps,\n    left_ukf_ps, right_ukf_ps, ekf_ps, iekf_ps)"
      ]
    },
    {
      "source": [
        "The novel retraction on $SE_2(3)$ resolves the problem encountered by\nthe $SO(3) \\times \\mathbb{R}^6$ UKF and particularly the EKF.\n\nWe confirm these plots by computing statistical results averaged over all the\nMonte-Carlo. We compute the Root Mean Squared Error (RMSE) for each method\nboth for the orientation and the position.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "model.benchmark_print(ukf_err, left_ukf_err, right_ukf_err, iekf_err, ekf_err)"
      ]
    },
    {
      "source": [
        "For the considered Monte-Carlo, we have first observed that EKF is not working\nvery well. Then, it happens that IEKF, left UKF and right UKF are the best\nin the first instants of the trajectory, that is confirmed with RMSE.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "We now compare the filters in term of consistency (NEES).\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "model.nees_print(ukf_nees, left_ukf_nees, right_ukf_nees, iekf_nees, ekf_nees)"
      ]
    },
    {
      "source": [
        "The $SO(3) \\times \\mathbb{R}^6$ UKF and EKF are too optimistic. Left\nUKF, right UKF and IEKF obtain similar NEES, UKFs are slightly better on the\nfirst secondes.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "**Which filter is the best ?** IEKF, **Left UKF** and **right UKF** obtain \nroughly similar accurate results, whereas these two UKFs are the more\nconsistent.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "Conclusion\n==============================================================================\nThis script compares different algorithms on the inertial navigation on flat\nEarth example. The left UKF and the right UKF, build on $SE_2(3)$\nretraction, outperform the EKF and seem slightly better than the IEKF.\n\nYou can now:\n\n- confirm (or infirm) the obtained results on massive Monte-Carlo\n  simulations. Another relevant comparision consists in testing the filters\n  when propagation noise is very low (standard deviation of $10^{-4}$),\n  as suggested in :cite:`barrauInvariant2017`.\n\n- address the problem of 2D SLAM, where the UKF is, among other, leveraged to\n  augment the state when a novel landmark is observed.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    }
  ],
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "name": "python3",
      "language": "python",
      "display_name": "Python 3"
    },
    "language_info": {
      "version": "3.5.2",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "pygments_lexer": "ipython3",
      "name": "python",
      "file_extension": ".py",
      "nbconvert_exporter": "python",
      "mimetype": "text/x-python"
    }
  },
  "nbformat": 4
}